/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-07     Hao       the first version
 */
#include <hardware/as5600.h>

#define  AS5600_ADDR                0x36
#define  AS5600_RAW_AGNLE_HI        0x0C
#define  AS5600_RAW_AGNLE_LO        0x0D
#define  AS5600_CPR                 4096

static struct rt_i2c_bus_device *i2c_bus = RT_NULL;

int as5600_init(void)
{
    i2c_bus = (struct rt_i2c_bus_device *)rt_device_find("i2c1");
    RT_ASSERT(i2c_bus != RT_NULL);

    return 0;
}
INIT_APP_EXPORT(as5600_init);

float as5600_getAngle()
{
    uint8_t temp[2] = {0, 0};

    // 发送命令
    rt_uint8_t buf = AS5600_RAW_AGNLE_HI;
    struct rt_i2c_msg msgs;
    msgs.addr = AS5600_ADDR;
    msgs.flags = RT_I2C_WR;
    msgs.buf = &buf;
    msgs.len = 1;
    if(1 != rt_i2c_transfer(i2c_bus, &msgs, 1))
    {
        return 0;
    }

    // 读取数据
    msgs.addr = AS5600_ADDR;
    msgs.flags = RT_I2C_RD;
    msgs.buf = temp;
    msgs.len = 2;
    if (1 != rt_i2c_transfer(i2c_bus, &msgs, 1))
    {
        return 0;
    }

    // 处理数据
    temp[0] = temp[0] & 0xF;

    return (float)((temp[0] << 8) + temp[1]) * 0.00153398f; // 1 / AS5600_CPR * _2PI = 0.00153398f
}

#if 0

void as5600_test()
{
    for(uint8_t i = 0; i <= 50; i++)
    {
        float angle = as5600_getAngle();
        rt_kprintf("as5600 = %d.%d \n", (int)(angle * 1000) / 1000, (int)(angle * 1000) % 1000);

        rt_thread_mdelay(200);
    }
}
MSH_CMD_EXPORT(as5600_test, as5600 test);

#endif
